package defectsim.integrator;

import javax.vecmath.Vector2d;

import repast.simphony.space.continuous.NdPoint;

import defectsim.Globals;
import defectsim.SimulationEnvironment;
import defectsim.agents.Dislocation;
import defectsim.agents.EdgeDislocation;
import defectsim.resources.AllForces;
import defectsim.resources.DefectResource.ForceType;

public class VelocityVerletIntegrator implements Integrator{
	
	private SimulationEnvironment env = SimulationEnvironment.getInstance();
	
	private Vector2d velocity;
	private Vector2d distance;
	private Vector2d acceleration;
	/*
	 * Virtual dislocation is used only for integration purposes in order to overcome 
	 * the consequences of iterative approach which requires a change in location 
	 * of the dislocation under integration.
	 */
	private Dislocation virtualDislocation;
	
	public VelocityVerletIntegrator(){
		virtualDislocation = new EdgeDislocation();
	}
	
	@Override
	public void integrate(Dislocation da, SimulationEnvironment env, Vector2d netForce, AllForces allForces) {
		// Velocity Verlet algorithm is applied for time integration of equations of motion
		virtualDislocation = da;
		double effectiveMass = 2.9e14;
		
		double force = netForce.x;
		
		double timeStep = env.getTimeStep();
		double v = da.getVelocity().x;
		double oldAcc = da.getAcceleration().x;
		double d = v * timeStep + oldAcc * timeStep * timeStep / 2.0;
		
		this.moveByVector(d * Globals.METER_TO_MICROMETER, Math.toRadians(virtualDislocation.getDirection()));
		Vector2d externalForce = allForces.getForce(ForceType.External).compute(virtualDislocation);
		Vector2d frictionForce = allForces.getForce(ForceType.Friction).compute(virtualDislocation);
		Vector2d interactionForce = allForces.getForce(ForceType.Interaction).compute(virtualDislocation);
		
		Vector2d effectiveForce = new Vector2d();
		effectiveForce.add(externalForce, interactionForce);
		if (effectiveForce.x > 0.0)
			netForce.sub(effectiveForce, frictionForce);
		else
			netForce.add(effectiveForce, frictionForce);
		
		force = netForce.x;
		double newAcc = force * effectiveMass;
//		acceleration = force + 5e-5 * v;
		v += (newAcc + oldAcc) * timeStep / 2.0;
		if(v == Double.POSITIVE_INFINITY){
			System.out.println("infinity found!");
			System.out.println(this.toString() + "Force: " + force);
			System.out.println(this.toString() + "Acc.: " + acceleration);
			System.out.println(this.toString() + "Dist: " + distance);
			System.out.println(this.toString() + "Velocity: " + velocity);
		}
		d *= Globals.METER_TO_MICROMETER;
		velocity = new Vector2d(v, 0.0);
		distance = new Vector2d(d, 0.0);
		acceleration = new Vector2d(newAcc, 0.0);

	}
	
	private void moveByVector(double d, double dir){
		double dx = d * Math.cos(dir);
		double dy = d * Math.sin(dir);
		NdPoint currentPoint = env.getLocation(virtualDislocation);
		virtualDislocation.setX(currentPoint.getX() + dx);
		virtualDislocation.setY(currentPoint.getY() + dy);
	}

	@Override
	public Vector2d getAcceleration() {
		return acceleration;
	}

	@Override
	public Vector2d getDistance() {
		return distance;
	}

	@Override
	public Vector2d getVelocity() {
		return velocity;
	}

}

// Velocity Verlet algorithm is applied for time integration of equations of motion
//			d = v * dt + acceleration * dt * dt / 2.0; 
//			double vtemp = v + acceleration * dt / 2.0;
//			acceleration = force * 2.9e16;
//			acceleration = force + 5e-5 * v;
//			v += vtemp + acceleration * dt / 2.0;
//			if(v == Double.POSITIVE_INFINITY){
//				System.out.println("infinity found!");
//				System.out.println(this.toString() + "Force: " + force);
//				System.out.println(this.toString() + "Acc.: " + acceleration);
//				System.out.println(this.toString() + "Dist: " + d);
//				System.out.println(this.toString() + "Vtemp: " + vtemp);
//			}
//			d *= 1e6;